/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: BehaviourData.cpp,v 1.6 2009-05-13 07:18:16 amaula Exp $
 *
 * \file
 * \brief Implementation for Behaviour data encoder/decoder
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "BehaviourData.hpp"
#include "binbag.h"
#include <stdio.h>
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Behaviour;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CBehaviourData::CBehaviourData()
  :iCommandPtr(NULL),
   iBehaviourControlPtr(NULL),
   iBehaviourControlSpeedPtr(NULL),
   iBehaviourInfoPtr(NULL),
   iBehaviourStatePtr(NULL){}
//*****************************************************************************

CBehaviourData::~CBehaviourData()
{

}
//*****************************************************************************
 
CBehaviourData::CBehaviourData(const CBehaviourData &aBehaviourData)
 : iCommandPtr(NULL),
   iBehaviourControlPtr(NULL),
   iBehaviourControlSpeedPtr(NULL),
   iBehaviourInfoPtr(NULL),
   iBehaviourStatePtr(NULL)
{
  *this = aBehaviourData;
}
//*****************************************************************************
     
CBehaviourData &CBehaviourData::operator=(const CBehaviourData &aBehaviourData)
{
  // Reset
  Reset();
  // Create an internal BinBag instance
  if (CreateInternalBinBag(aBehaviourData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aBehaviourData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"WARNING: Failed to Decode BehaviourData even though assignment operator used?");
  }

  return *this; 
}
//*****************************************************************************
   
bool CBehaviourData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
 bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeCommand:
        if (iCommandPtr == NULL) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data);
        } else {

	  result = false;
	}
        break;

      case KTypeBehaviourControl:
        if(iBehaviourControlPtr == NULL){
          iBehaviourControlPtr = reinterpret_cast<const TBehaviourControl *>(b->data);
        } else {

	  result = false;
	}
        break;

      case KTypeBehaviourControlSpeed:
        if(iBehaviourControlSpeedPtr == NULL){
          iBehaviourControlSpeedPtr = reinterpret_cast<const TBehaviourControlSpeed *>(b->data);
        } else {

	  result = false;
	}
        break;

      case KTypeBehaviourInformation:
         if(iBehaviourInfoPtr == NULL){
          iBehaviourInfoPtr = reinterpret_cast<const TBehaviourInformation *>(b->data);
        } else {

	  result = false;
	}
        break;
      case KTypeBehaviourState:
         if(iBehaviourStatePtr == NULL){
          iBehaviourStatePtr = reinterpret_cast<const TBehaviourState *>(b->data);
        } else {

	  result = false;
	}
        break;
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
      // Store previous type
      previous_type = b->type;
    }
  } else result = false;
  
  // Store the result
  SetReadOnlyContainer(result);
  
  return result;
}
//*****************************************************************************      
    
void CBehaviourData::Reset()
{
  CMaCIData::Reset();

  iCommandPtr = NULL;
  iBehaviourControlPtr = NULL;
  iBehaviourControlSpeedPtr = NULL;
  iBehaviourInfoPtr = NULL;
  iBehaviourStatePtr = NULL;
}
//*****************************************************************************
     
bool CBehaviourData::SetCommand(const TCommand &aCommand)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCommand);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CBehaviourData::SetBehaviourControl(const TBehaviourControl &aControl)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iBehaviourControlPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TBehaviourControl, KTypeBehaviourControl, iBehaviourControlPtr, aControl);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CBehaviourData::SetBehaviourControlSpeed(const TBehaviourControlSpeed &aControl)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iBehaviourControlSpeedPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TBehaviourControlSpeed, KTypeBehaviourControlSpeed, iBehaviourControlSpeedPtr, aControl);
    result = true;
  }
  return result;

}
//*****************************************************************************
bool CBehaviourData::SetBehaviourInfo(const TBehaviourInformation &aInfo)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iBehaviourInfoPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TBehaviourInformation, KTypeBehaviourInformation, iBehaviourInfoPtr, aInfo);
    result = true;
  }
  return result;

}
//*****************************************************************************

bool CBehaviourData::SetBehaviourState(const TBehaviourState &aInfo)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iBehaviourStatePtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TBehaviourState, KTypeBehaviourState, iBehaviourStatePtr, aInfo);
    result = true;
  }
  return result;



}
//*****************************************************************************

void CBehaviourData::Print(const int level) const
{
 const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeCommand: {
        const TCommand *cp = reinterpret_cast<const TCommand *>(b->data);
        dPrint(level,"TCommand: %d",
               cp->cmd);
        break;
      }
      case KTypeBehaviourControl:{
        const TBehaviourControl *bc = reinterpret_cast<const TBehaviourControl *>(b->data);
        dPrint(level,"TBehaviourControl: speed %f, angle %f",
               bc->speed,
               bc->angle);

        break;
      }

      case KTypeBehaviourControlSpeed:{
        const TBehaviourControlSpeed *bc = reinterpret_cast<const TBehaviourControlSpeed *>(b->data);
        dPrint(level,"TBehaviourControlSpeed: speed %f, angularSpeed %f",
               bc->speed,
               bc->angularSpeed);
        
        break;
      }

      case KTypeBehaviourInformation:{
        const TBehaviourInformation *bi = reinterpret_cast<const TBehaviourInformation *>(b->data);
        dPrint(level,"TBehaviourInformation: BehaviourType %d",
               bi->type);

        break;

      }
      case KTypeBehaviourState:{
        const TBehaviourState *bi = reinterpret_cast<const TBehaviourState *>(b->data);
        if(bi->state == 0){
          dPrint(level,"TBehaviourState: Behaviour is not running");
        }else{
          dPrint(level,"TBehaviourState: Behaviour is running");
        }
        break;
        

      }
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
  
  
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_BEHAVIOURDATA_STANDALONE_MAIN
using namespace MaCI::Behaviour;

int main(void){
  debugInit();
  gim::binbag::CBinBag bb;
  float speed = 11;
  float angle = 3.14;
  float controlSpeed = 4.2;
  float angularSpeed = 1.11;
  CBehaviourData A;
  CBehaviourData B;
  printf("Carry out Encoding/Decoding test.\n");
  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  printf("Encode: Setting Command(Start (%d))\n",KCommandStart);
  A.SetCommand(TCommand(KCommandStart));
  printf("OK.\n");

  //NOTE: normally it's not OK to put start command and a behaviourControl structure in BehaviourData.
  //      This is only for demonstrating and testing to put different structures to BehaviourData
  printf("Encode: Setting BehaviourControl(Speed %f, angle %f)\n",speed,angle);
  A.SetBehaviourControl(TBehaviourControl(speed,angle));
  printf("OK.\n"); 

  printf("Encode: Setting BehaviourControlSpeed(Speed %f, angularSpeed %f\n",controlSpeed,angularSpeed);
  A.SetBehaviourControlSpeed(TBehaviourControlSpeed(controlSpeed,angularSpeed));
  printf("OK.\n");

  printf("Encode: Setting BehaviourInformation(type:%d)\n",KBehaviourTypeNormal);
  A.SetBehaviourInfo(TBehaviourInformation(KBehaviourTypeNormal));
  printf("OK.\n"); 

  printf("Encode: Setting BehaviourState to not running (0)\n");
  A.SetBehaviourState(TBehaviourState(0));
    printf("OK.\n"); 
  printf("Let's print it:\n");
  A.Print(1);

  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  printf("Decoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);

  printf("Decode: Command(%d)\n",B.GetCommand()->cmd);

  printf("Decode: Behaviourcontrol (speed %f, angle %f)\n",B.GetBehaviourControl()->speed,B.GetBehaviourControl()->angle);
  printf("Decode: BehaviourcontrolSpeed (speed %f, angularSpeed %f)\n",B.GetBehaviourControlSpeed()->speed,B.GetBehaviourControlSpeed()->angularSpeed);
  printf("Decode: BehaviourInfo(%d)\n",B.GetBehaviourInfo()->type);
  printf("Decode: BehaviourState(%d)\n",B.GetBehaviourState()->state);
  printf("Then try the Print function\n");
  B.Print(1);
  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  CBehaviourData C (A);
  printf("Trying the copy constructor..\n");
  printf("Print data:\n");
  C.Print(1);
  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  CBehaviourData D = C;
  printf("Trying the = operator\n");
  printf("Print data:\n");
  D.Print(1);
  printf("\nTest complete.Bye Bye!\n");


  return 0;

}
#endif






